#include "GooMFInstrumentor.h"
#include "buffer.h"
#include "rithmic_globals.h"
#include <iostream>
#include "stdio.h"
#include<string.h>


#include "tsp_measurement.h"
#include "tsp_measurement_cmd.h"

#include "vxWorks.h"
#include "taskLib.h"
#include "msgQLib.h"
#include "sysLib.h"

#include "tcs_msg.h"
#include "tcs_struct.h"
#include "tss_typedef.h"
#include "tss_error.h"


extern MSG_Q_ID msgTcsServer;

TSS_RESULT cMeasurement::getHardwareInfo(OUT  PCPUINFO p){

	TSS_RESULT flag = TSP_MEA_SUCCESS;
	/*设置结果接收队列的长度为1*/
	numMsg = 1;
	
	/*参数检查*/
	if(p == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	
	/*申请存放命令结构体的内存*/
	pMea = (TrustReq*)calloc(1,sizeof(TrustReq));
	if(pMea == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
		
	/*========================================
	========初始化可信命令结构体==============
	==========================================*/			
	
	/*构造可信命令结构体*/
	pMea->code = TSP_MEA_GET_HARDWARE;
	/*该命令不含参数，所以为空*/
	pMea->param = NULL;
	/*获取命令优先级*/
	priority = getPriority();	
	/*创建可信命令优先级以及结果返回队列*/	
	if((msgResponse = msgQCreate(numMsg,sizeof(struct CPUInfo),
		MSG_Q_FIFO)) == NULL){
		std::cout<<"Error in create msgQ"<<endl;
		flag = TSP_NULL_QUEUE;
		return flag;
	}
	/*将可信命令封装为TCS队列节点所需的结构体*/
	node.msgResponse = msgResponse;
	node.priority =	priority ;
	node.trustAskSt = pMea;

	
	/*将封装好的结构体发送至TCS的消息接收队列msgTcsServer*/
	if((msgQSend(msgTcsServer,(char*)&node,sizeof(struct MSG_Ask),
		WAIT_FOREVER,MSG_PRI_NORMAL))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in sending the msg, error ID = %d\n", flag);
			return flag;
	}
	/*从结果反馈队列中读取结果*/
	if((msgQReceive(msgResponse,(char*)p,sizeof(struct CPUInfo),
		WAIT_FOREVER))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in receiving msg, error ID  = %d\n", flag);
			return flag;
			
	}
	/*清理操作*/	
	free(pMea);
	pMea = NULL;
	msgQDelete(msgResponse);
	return flag;
}

TSS_RESULT cMeasurement::getHardwareInfo(IN char* ip, OUT PCPUINFO p){

	TSS_RESULT flag = TSP_MEA_SUCCESS;
	/*声明TCP通信所需结构体*/
	int					sFd;
    struct TrustReq     clientReq;     
    struct sockaddr_in  serverAddr;      
    int                 sockAddrSize; 
	
	MSG_CmdInfo_Mea 	cmdInfo_mea;
	UINT32				hardwareFlag;
	
	/*参数检查*/
	if(p == NULL || ip == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	/*===================================
	==============socket通信=============
	=====================================*/
    if ((sFd = socket (AF_INET, SOCK_STREAM, 0)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("socket create failed error ID = %d\n", flag); 
		return flag;
    } 
  
    sockAddrSize = sizeof (struct sockaddr_in); 
    bzero ((char *) &serverAddr, sockAddrSize); 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_len = (u_char) sockAddrSize; 
    serverAddr.sin_port = htons (SERVER_PORT_NUM); 
 
    if (((serverAddr.sin_addr.s_addr = inet_addr (ip)) == ERROR) && 
        ((serverAddr.sin_addr.s_addr = hostGetByName (ip)) == ERROR)){ 
        flag = TSP_FAIL_TCP;
		printf("client set unknown server name, error ID = %d\n", flag); 
		close (sFd); 
		return flag;
    } 
 
    if (connect (sFd, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("client connect failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    } 
	
	cmdInfo_mea.code = TSP_MEA_GET_HARDWARE;
	cmdInfo_mea.length = 0;
	cmdInfo_mea.priority = getPriority();
	
	hardwareFlag = 0;
	
    if (write (sFd, (char *) &cmdInfo_mea, sizeof (struct MSG_CmdInfo_Mea)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("write to server failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    }
	
	if(read(sFd,(char*)&hardwareFlag, sizeof(UINT32)) == ERROR){
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);
	}
	
	if( hardwareFlag == 0){
		flag = TSP_FAIL_TCP;
		printf("cmd code info error, error ID = %d\n ",flag);
		return flag;
		close(sFd);
	}
	
	if (read (sFd, (char*)p, sizeof (struct CPUInfo)) < 0) { 
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);        
    }
    close (sFd);
    return flag; 	
}


TSS_RESULT cMeasurement::getSoftwareInfo(OUT  PSOFTINFO p){

	TSS_RESULT flag = TSP_MEA_SUCCESS;
	
	numMsg = 1;
	/*参数检查*/
	if(p == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	
	pMea = (TrustReq*)calloc(1,sizeof(TrustReq));
	if(pMea == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
		
	/*========================================
	========初始化可信命令结构体==============
	==========================================*/			
	
	/*构造可信命令结构体*/
	pMea->code = TSP_MEA_GET_SOFTWARE;
	pMea->param = NULL;
	/*创建可信命令优先级以及结果返回队列*/
	
	priority = getPriority();		
	if((msgResponse = msgQCreate(numMsg,sizeof(struct SoftInfo),
		MSG_Q_FIFO)) == NULL){
		std::cout<<"Error in create msgQ"<<endl;
		flag = TSP_NULL_QUEUE;
		return flag;
	}
	
	/*========================================
	================发送可信命令===============
	==========================================*/
	

	node.msgResponse = msgResponse;
	node.priority =	priority ;
	node.trustAskSt = pMea;
	
	
	/*focus on the value msgQId_S, that's a public message queue created by tcs*/
	if((msgQSend(msgTcsServer,(char*)&node,sizeof(struct MSG_Ask),
		WAIT_FOREVER,MSG_PRI_NORMAL))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in sending the msg, error ID = %d\n", flag);
			return flag;
	}
	/*========================================
	================接收可信命令===============
	==========================================*/
	if((msgQReceive(msgResponse,(char*)p,sizeof(struct SoftInfo),
		WAIT_FOREVER))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in receiving msg, error ID  = %d\n", flag);
			return flag;
			
		}
		
	free(pMea);
	pMea = NULL;
	msgQDelete(msgResponse);
	return flag;
}

TSS_RESULT cMeasurement::getSoftwareInfo(IN char* ip, OUT PSOFTINFO p){

	TSS_RESULT flag = TSP_MEA_SUCCESS;
	/*声明TCP通信所需结构体*/
	int					sFd;
    struct TrustReq     clientReq;     
    struct sockaddr_in  serverAddr;      
    int                 sockAddrSize; 
	
	MSG_CmdInfo_Mea		cmdInfo_mea;
	UINT32				softwareFlag;
	
	/*参数检查*/
	if(p == NULL || ip == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	/*===================================
	==============socket通信=============
	=====================================*/
    if ((sFd = socket (AF_INET, SOCK_STREAM, 0)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("socket create failed error ID = %d\n", flag); 
		return flag;
    } 
  
    sockAddrSize = sizeof (struct sockaddr_in); 
    bzero ((char *) &serverAddr, sockAddrSize); 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_len = (u_char) sockAddrSize; 
    serverAddr.sin_port = htons (SERVER_PORT_NUM); 
 
    if (((serverAddr.sin_addr.s_addr = inet_addr (ip)) == ERROR) && 
        ((serverAddr.sin_addr.s_addr = hostGetByName (ip)) == ERROR)){ 
        flag = TSP_FAIL_TCP;
		printf("client set unknown server name, error ID = %d\n", flag); 
		close (sFd); 
		return flag;
    } 
 
    if (connect (sFd, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("client connect failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    } 
	
	cmdInfo_mea.code = TSP_MEA_GET_SOFTWARE;
	cmdInfo_mea.length = 0;
	cmdInfo_mea.priority = getPriority();
	softwareFlag = 0;
    if (write (sFd, (char *) &cmdInfo_mea, sizeof (struct MSG_CmdInfo_Mea)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("write to server failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    }
	
	if(read(sFd,(char*)&softwareFlag, sizeof(UINT32)) == ERROR){
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);
	}
	
	if( softwareFlag == 0){
		flag = TSP_FAIL_TCP;
		printf("cmd code info error, error ID = %d\n ",flag);
		return flag;
		close(sFd);
	}
	
	if (read (sFd, (char*)p, sizeof (struct SoftInfo)) < 0) { 
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);        
    }
    close (sFd);
    return flag; 	
}

TSS_RESULT cMeasurement::programVerify(	IN 		UINT32 		length, 
										IN 		BYTE* 		parameter){
	
	TSS_RESULT flag = TSP_MEA_SUCCESS;
	int i, index;
    UINT8* p;
	numMsg = 1;
	/*参数检查*/
	if( parameter == NULL ){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	printf("TSP: parameter address = %x\n",parameter);
	/*将第一个参数压入参数数组*/
	index = 0;
    for(i = 0; i < 4; i++) {
        parameterBuffer[index++] = length>>((3-i)*8);
    }
	/*将第二个参数的指针压入参数数组*/
	for(i = 0; i < 4; i++) {
        parameterBuffer[index++] = (UINT32)parameter>>((3-i)*8);
    }
	
	
	pMea = (TrustReq*)calloc(1,sizeof(TrustReq));
	if(pMea == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
		
	/*========================================
	========初始化可信命令结构体==============
	==========================================*/			
	
	/*构造可信命令结构体*/
	pMea->code = TSP_MEA_PROGRAM_VERIFY;
	pMea->param = parameterBuffer;
	/*创建可信命令优先级以及结果返回队列*/
	
	priority = getPriority();		
	if((msgResponse = msgQCreate(numMsg,sizeof(TSS_RESULT),
		MSG_Q_FIFO)) == NULL){
		std::cout<<"Error in create msgQ"<<endl;
		flag = TSP_NULL_QUEUE;
		return flag;
	}
	
	/*========================================
	================发送可信命令===============
	==========================================*/
	

	node.msgResponse = msgResponse;
	node.priority =	priority ;
	node.trustAskSt = pMea;
	
	/*focus on the value msgQId_S, that's a public message queue created by tcs*/
	if((msgQSend(msgTcsServer,(char*)&node,sizeof(struct MSG_Ask),
		WAIT_FOREVER,MSG_PRI_NORMAL))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in sending the msg, error ID = %d\n", flag);
			return flag;
	}
	/*========================================
	================接收可信命令===============
	==========================================*/
	if((msgQReceive(msgResponse,(char*)&flag,sizeof(TSS_RESULT),
		WAIT_FOREVER))==ERROR){
			flag = TDDL_NULL_QUEUE;
			printf("Error in receiving msg, error ID  = %d\n", flag);
			return flag;
			
	}
	
	if( flag == TDDL_SUCCESS){
		flag =  TSP_MEA_SUCCESS;
	}else{	
		flag = TSP_PROGRAM_FAIL;
	}
	
	free(pMea);
	pMea = NULL;
	msgQDelete(msgResponse);
	return flag;
	
}

TSS_RESULT cMeasurement::programVerify(IN UINT32 length, IN BYTE* parameter, IN char* ip){

	TSS_RESULT flag = TSP_MEA_SUCCESS;
	/*声明TCP通信所需结构体*/
	int					sFd;
    struct TrustReq     clientReq;     
    struct sockaddr_in  serverAddr;      
    int                 sockAddrSize; 
	
	/*命令相关信息结构体*/
	MSG_CmdInfo_Mea		cmdInfo_mea;
	/*命令相关信息接收标识位*/
	UINT32				programFalg;
	
	/*参数检查*/
	if(parameter == NULL || ip == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	/*===================================
	==============socket通信=============
	=====================================*/
    if ((sFd = socket (AF_INET, SOCK_STREAM, 0)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("socket create failed error ID = %d\n", flag); 
		return flag;
    } 
  
    sockAddrSize = sizeof (struct sockaddr_in); 
    bzero ((char *) &serverAddr, sockAddrSize); 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_len = (u_char) sockAddrSize; 
    serverAddr.sin_port = htons (SERVER_PORT_NUM); 
 
    if (((serverAddr.sin_addr.s_addr = inet_addr (ip)) == ERROR) && 
        ((serverAddr.sin_addr.s_addr = hostGetByName (ip)) == ERROR)){ 
        flag = TSP_FAIL_TCP;
		printf("client set unknown server name, error ID = %d\n", flag); 
		close (sFd); 
		return flag;
    } 
 
    if (connect (sFd, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("client connect failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    } 
	
	
	/*发送命令相关信息*/
	cmdInfo_mea.code = TSP_MEA_PROGRAM_VERIFY;
	cmdInfo_mea.priority = getPriority();
	cmdInfo_mea.length = length;
	/*0 表示 当前处于初始状态，1代表TCS准备好接收命令*/
	programFalg = 0;
	
    if (write (sFd, (char *) &cmdInfo_mea, sizeof (struct MSG_CmdInfo_Mea)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("write to server failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    }
	
	
	if (read (sFd, (char*)&programFalg, sizeof (UINT32)) < 0) { 
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);        
    }
	
	if( programFalg == 0){
		flag = TSP_FAIL_TCP; 
        printf ("cmd code error, error ID = %d\n", flag);
		return flag;
        close (sFd);   
	}
	
	if(write(sFd,(char*)parameter, length) == ERROR){
		flag = TSP_FAIL_TCP;
		printf("send program error, error id = %d\n", flag);
		return flag;
		close(sFd);
	}
	
	if(read(sFd,(char*)&flag,sizeof(TSS_RESULT)) == ERROR){
		flag = TSP_FAIL_TCP;
		printf("get program verify result error, error id = %d\n", flag);
		return flag;
		close(sFd);
	
	}
	
	if( flag == TDDL_SUCCESS){
		flag =  TSP_MEA_SUCCESS;
	}else{	
		flag = TSP_PROGRAM_FAIL;
	}
	
    close (sFd);
    return flag; 	
}

TSS_RESULT cMeasurement::platformVerify(IN PCPUINFO pCpuInfo, IN PSOFTINFO pSoftInfo){
	
	TSS_RESULT flag = TSP_MEA_SUCCESS;
	
	int i, index;
	numMsg = 1;
	
	/*参数检查*/
	if( pCpuInfo == NULL || pSoftInfo == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}

	/*将第一个参数压入参数数组*/
	index = 0;
    for(i = 0; i < 4; i++) {
        parameterBuffer[index++] = (UINT32)pCpuInfo>>((3-i)*8);
    }
	/*将第二个参数压入参数数组*/
	for(i = 0; i < 4; i++) {
        parameterBuffer[index++] = (UINT32)pSoftInfo>>((3-i)*8);
    }
		
	pMea = (TrustReq*)calloc(1,sizeof(TrustReq));
	if(pMea == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
		
	/*========================================
	========初始化可信命令结构体==============
	==========================================*/			
	
	/*构造可信命令结构体*/
	pMea->code = TSP_MEA_PLATFORM_VERIFY;
	pMea->param = parameterBuffer;
	/*创建可信命令优先级以及结果返回队列*/
	
	priority = getPriority();		
	if((msgResponse = msgQCreate(numMsg,sizeof(TSS_RESULT),
		MSG_Q_FIFO)) == NULL){
		std::cout<<"Error in create msgQ"<<endl;
		flag = TSP_NULL_QUEUE;
		return flag;
	}
	
	/*========================================
	================发送可信命令===============
	==========================================*/
	

	node.msgResponse = msgResponse;
	node.priority =	priority ;
	node.trustAskSt = pMea;

	/*focus on the value msgQId_S, that's a public message queue created by tcs*/
	if((msgQSend(msgTcsServer,(char*)&node,sizeof(struct MSG_Ask),
		WAIT_FOREVER,MSG_PRI_NORMAL))==ERROR){
			flag = TSP_NULL_QUEUE;
			printf("Error in sending the msg, error ID = %d\n", flag);
			return flag;
	}
	/*========================================
	================接收可信命令===============
	=========================================*/
	if((msgQReceive(msgResponse,(char*)&flag,sizeof(TSS_RESULT),
		WAIT_FOREVER))==ERROR){
			flag = TDDL_NULL_QUEUE;
			printf("Error in receiving msg, error ID  = %d\n", flag);
			return flag;
			
	}
	
	if( flag == TDDL_SUCCESS){
		flag =  TSP_MEA_SUCCESS;
	}else{	
		flag = TSP_PLATFORM_FAIL;
	}
	
	free(pMea);
	pMea = NULL;
	msgQDelete(msgResponse);
	return flag;
	
}

TSS_RESULT cMeasurement::platformVerify(IN PCPUINFO pCpuInfo, IN PSOFTINFO pSoftInfo, IN char* ip){
	TSS_RESULT flag = TSP_MEA_SUCCESS;
	
	/*声明TCP通信所需结构体*/
	int					sFd;
    struct TrustReq     clientReq;     
    struct sockaddr_in  serverAddr;      
    int                 sockAddrSize; 
	
	/*命令相关信息结构体*/
	MSG_CmdInfo_Mea		cmdInfo_mea;
	/*命令相关信息接收标识位*/
	UINT32				programFalg;
	
	/*参数检查*/
	if(pCpuInfo == NULL || pSoftInfo == NULL || ip == NULL){
		flag = TSP_NULL_POINTER;
		return flag;
	}
	/*===================================
	==============socket通信=============
	=====================================*/
    if ((sFd = socket (AF_INET, SOCK_STREAM, 0)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("socket create failed error ID = %d\n", flag); 
		return flag;
    } 
  
    sockAddrSize = sizeof (struct sockaddr_in); 
    bzero ((char *) &serverAddr, sockAddrSize); 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_len = (u_char) sockAddrSize; 
    serverAddr.sin_port = htons (SERVER_PORT_NUM); 
 
    if (((serverAddr.sin_addr.s_addr = inet_addr (ip)) == ERROR) && 
        ((serverAddr.sin_addr.s_addr = hostGetByName (ip)) == ERROR)){ 
        flag = TSP_FAIL_TCP;
		printf("client set unknown server name, error ID = %d\n", flag); 
		close (sFd); 
		return flag;
    } 
 
    if (connect (sFd, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("client connect failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    } 
	
	
	/*发送命令相关信息*/
	cmdInfo_mea.code = TSP_MEA_PLATFORM_VERIFY;
	cmdInfo_mea.priority = getPriority();
	cmdInfo_mea.length = 0;
	/*0 表示 当前处于初始状态，1代表TCS准备好接收命令*/
	programFalg = 0;
	
    if (write (sFd, (char *)&cmdInfo_mea, sizeof (struct MSG_CmdInfo_Mea)) == ERROR){ 
		flag = TSP_FAIL_TCP; 
        printf ("write to server failed, error ID = %d\n", flag); 
        close (sFd); 
		return flag;
    }
	
	
	if (read (sFd, (char*)&programFalg, sizeof (UINT32)) < 0) { 
		flag = TSP_FAIL_TCP; 
        printf ("read result fail, error ID = %d\n", flag);
		return flag;
        close (sFd);        
    }
	
	if( programFalg == 0){
		flag = TSP_FAIL_TCP; 
        printf ("cmd code error, error ID = %d\n", flag);
		return flag;
        close (sFd);   
	}
	
	/*将硬件信息写入流*/
	if(write(sFd,(char*)pCpuInfo, sizeof(struct CPUInfo)) == ERROR){
		flag = TSP_FAIL_TCP;
		printf("send program error, error id = %d\n", flag);
		return flag;
		close(sFd);
	}
	/*将软件信息写入流*/
	if(write(sFd,(char*)pSoftInfo, sizeof(struct SoftInfo)) == ERROR){
		flag = TSP_FAIL_TCP;
		printf("send program error, error id = %d\n", flag);
		return flag;
		close(sFd);
	}
	
	if(read(sFd,(char*)&flag,sizeof(TSS_RESULT)) == ERROR){
		flag = TSP_FAIL_TCP;
		printf("get program verify result error, error id = %d\n", flag);
		return flag;
		close(sFd);
	
	}
	
	if( flag == TDDL_SUCCESS){
		flag =  TSP_MEA_SUCCESS;
	}else{	
		flag = TSP_PLATFORM_FAIL;
	}
	
    close (sFd);
    return flag; 	
}
		
UINT32 cMeasurement::getPriority(){
	return 1;
}


/*RITHM-includes*/
#include<signal.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <pthread.h>    /* POSIX Threads */
#include <sched.h>
#include <stdlib.h>
#include "Controller.h"
#include "fuzzy.h"
#include "pid.h"

int bufftrig_count = 0;
int samptrig_count = 0;
//struct timespec start_nano[CONNMAX], end_nano[CONNMAX];
long elapsed_seconds;
struct timespec sleep_time;
pthread_mutex_t lock;
int initial_BufferSize;
int initial_Safety;
float controller_error;
float avg_eventcount;
float avg_samplingperiod;
float avg_deltatime;
struct rusage usage1, usage2;
_GOOMF_context context;

int kill_monitor = 0;

typedef struct _program_thread_args {
    int argc;
    char **argv;
} program_thread_args;

/* prototype for thread routine */
void monitor_thread ( );
void main_thread ( );
void report();
void runMonitor(int bufferTriggered);
int initContext(){
return _GOOMF_initContext(&context,_GOOMF_enum_buffer_trigger,_GOOMF_enum_alg_seq,_GOOMF_enum_sync_invocation,MAX_BS
);
};

int  destroyContext(){
return _GOOMF_destroyContext(&context);
};

int queueEvent(struct _GOOMF_host_program_state_struct event, long line_num) {
	if (queue_event(event,line_num) == -1)
	{
		runMonitor(1);
	}
}
/*RITHM-monitor-call*/
//-----------------------------------------------------------------------------------------------------------
void runMonitor(int bufferTriggered) {
	timespec current_time,delta_time;
	long timeElapsed;
	int bs_delta =  0;
    clock_gettime(CLOCK_REALTIME, &current_time);
    delta_time = diff(last_timestamp, current_time);

    timeElapsed = delta_time.tv_sec*1000000+delta_time.tv_nsec/1000;
    last_timestamp = current_time;
    controller_error = (float)(BUFF_SIZE)*(safety)/100-(float)(event_count);

    avg_err = (avg_err*sample_count + BUFF_SIZE_REAL - event_count);
    avg_eventcount = avg_eventcount*sample_count + event_count;
    avg_samplingperiod = avg_samplingperiod*sample_count + sampling_period;
    avg_deltatime = avg_deltatime*sample_count+timeElapsed;
    sample_count++;
    avg_err /= sample_count;
    avg_eventcount /= sample_count;
    avg_samplingperiod /= sample_count;
    avg_deltatime /= sample_count;
    sample_bt += bufferTriggered;


    if (sample_count == FREQ) {
    	bs_delta = getBufferSize();

    	if (BUFF_SIZE_REAL+bs_delta>100)
    		bs_delta = 100-BUFF_SIZE_REAL;
    	if (BUFF_SIZE_REAL+bs_delta<initial_BufferSize)
    		bs_delta = initial_BufferSize-BUFF_SIZE_REAL;

    	BUFF_SIZE_REAL += bs_delta;
    	safety = (int)((float)initial_BufferSize*(float)initial_Safety/(float)BUFF_SIZE_REAL);
    	BUFF_SIZE=BUFF_SIZE_REAL;

    	PRINTDEBUG("|\n|\n|\nBuffer Size is %i, safety %% is %i and val is %i\n|\n|\n", BUFF_SIZE_REAL,safety,safety*BUFF_SIZE/100);
    	//sample_count = 0;
    	avg_err = 0;
    	sample_bt = 0;
    	delta_time.tv_sec = timeElapsed/1000000;
    	delta_time.tv_nsec = (timeElapsed%1000000)*1000;
    	switch (controller_type) {
    	    case 1:
    	    	PID(controller_error, delta_time, bufferTriggered);
    	    	break;
    	    case 2:
    	    	sampling_period = getSamplingPeriodForError((float)avg_eventcount,delta_time, 0);
    	    	break;
    	    case 3:
    	    	sampling_period = getSamplingPeriodForError2((float)avg_eventcount,delta_time, 0);
    	    	break;
    	    case 4:
    	    case 5:
    	    	sampling_period = getSamplingPeriodForError3((float)event_count,delta_time, 0);
    	    	break;
    	    }
    	sample_count = 0;
    }


	sleep_time.tv_nsec = (sampling_period%1000000)*1000;
	sleep_time.tv_sec = sampling_period/1000000;
	if (bufferTriggered)
		bufftrig_count++;
	else
		samptrig_count++;
	PRINTDEBUG("Sampling Period is %li, Sleep Time is %ld:%ld\n", sampling_period, sleep_time.tv_sec, sleep_time.tv_nsec);
	//PRINTDEBUG("SP: %12li, %i events, error = %i\n", sampling_period, event_count, error);
	if (bufferTriggered)
		PRINTDEBUG("BUFFER TRIGGERED-(BUFFCOUNT:%i, SAMPCOUNT:%i)-)\n", bufftrig_count, samptrig_count);
	PRINTRT("%f\t%li\n", (float)(event_count)/(float)(BUFF_SIZE)*(safety)/100, timeElapsed);
	if (fp!=0) {
		fflush(fp);
		fseek(fp,0,SEEK_SET);
	}
	fflush(stdout);
	flush_buffer();
	report();
}


/*RITHM-monitor-code*/
//-----------------------------------------------------------------------------------------------------------

void main_thread (void *program_main_data)
{
	int i,j,k,y;
	struct sched_param param;
        program_thread_args *args = (program_thread_args *)program_main_data;

	int policy = SCHED_OTHER;
	/* scheduling parameters of target thread */
	pthread_setschedparam(pthread_self(), policy, &param);
    
    int *retVal = malloc(sizeof(int));
    if (!retVal) {
        fprintf(stderr, "main_thread() malloc failed! Aborting...\n");
        pthread_exit(0);
    }
    *retVal = 0;
	// Main thread Body
    if (!args) { *retVal = 1; pthread_exit(retVal); }
    *retVal =     program_main();

    kill_monitor = 1;
    pthread_exit((void *)retVal);
	
}
void monitor_thread ()
{
	struct timeval start, end;
	struct timespec start_nano, end_nano;
	int i = 0,j=0;

	// super user only -- switch to real-time mode and lock memory pages
	struct sched_param param;

	int policy = SCHED_FIFO;
	/* scheduling parameters of target thread */
	pthread_setschedparam(pthread_self(), policy, &param);
	(mlockall(MCL_CURRENT | MCL_FUTURE));

	setbuf(stdout, NULL);

	sleep_time.tv_nsec = (sampling_period%1000000)*1000;
	sleep_time.tv_sec = sampling_period/1000000;

	while(1) {
		clock_gettime(CLOCK_REALTIME, &start_nano);
		clock_nanosleep(CLOCK_REALTIME, 0, &sleep_time, NULL);
		runMonitor(0);
		//clock_gettime(CLOCK_REALTIME, &end_nano);
		PRINTDEBUG("Sleep Time is %ld:%ld\n", sleep_time.tv_sec, sleep_time.tv_nsec);
	}

	pthread_exit(0); /* exit */
}

int main(int argc, char** argv)
{
	pthread_t thread1, thread2;  /* thread variables */
	cpu_set_t  mask;
    program_thread_args *args = malloc(sizeof(program_thread_args));
    args->argc = argc;
    args->argv = argv;
	initial_BufferSize = BUFF_SIZE;
	BUFF_SIZE_REAL = BUFF_SIZE;
	initial_Safety = safety;
	
	fp=fopen("/tmp/rithm","w");



    buffer_init();
    initController();
    initFuzzy(controller_type >= 5);
	
	// This is just a switch to run the program in hi-res timer evaluation mode
	// If checktimers() is called it will output the average of a timer with a given interval
	if (argc >= 2 && !strcmp(argv[1],"-test")) {
		printf("%s\n", argv[1]);
	}
	else {
		/* create monitor and main thread */
		pthread_create(&thread1, NULL, (void *) &monitor_thread, 0);
		pthread_create(&thread2, NULL, (void *) &main_thread, (void *)args);

		/* Main block now waits for both threads to terminate, before it exits
   If main block exits, both threads exit, even if the threads have not
   finished their work */
		pthread_join(thread2, NULL);
		//pthread_join(thread1, NULL);
	}

	/* exit */
    free(args);
     buffer_end();
	exit(0);
} /* main() */




void report(){ 
	_GOOMF_enum_verdict_type v[1];
	char str[100]; 
	register int i; 
	_GOOMF_getCurrentStatus(context, v); 
	_GOOMF_typeToString(&v[0], str); 
	fprintf(logFile,	"prop1: %s\n", str); 

}; 

